;---------------------------------------------------------------------------;
; USI control functions
;---------------------------------------------------------------------------;

.nolist
#include <avr/io.h>	// Include device specific definitions.
.list

#define	_FLAGS	_SFR_IO_ADDR(GPIOR0)

#define B_DO    4

;---------------------------------------------------------------------------;
; Simple Delay
;---------------------------------------------------------------------------;
; void delay_ms(WORD ms);
; void delay_us(WORD us);

.global delay_ms
.func delay_ms
delay_ms:
	wdr			; Reset WDT
	sbiw	r24, 1		; if (ms-- == 0) return;
	brcs	9f		; /
	ldi	ZL, lo8(4000)	; 1ms delay (16MHz)
	ldi	ZH, hi8(4000)	;
1:	sbiw	ZL, 1		;
	brne	1b		; /
	rjmp	delay_ms
9:	ret
.endfunc


.global delay_us
.func delay_us
delay_us:
	ldi	r23, 2
1:	dec	r23
	brne	1b
	sbiw	r24, 1
	brne	delay_us
	wdr
	ret
.endfunc


;---------------------------------------------------------------------------;
; Initialize SPI
;
; void init_spi (void);

.global init_spi
.func init_spi
init_spi:
    ldi r24, 0b01010000
    out _SFR_IO_ADDR(SPCR), r24
    sbi _SFR_IO_ADDR(PORTB), B_DO ; DO = H
	ret
.endfunc

;---------------------------------------------------------------------------;
; Transmit a byte
;
; BYTE xmit_spi (BYTE);

.global xmit_spi
.func xmit_spi
xmit_spi:
    out _SFR_IO_ADDR(SPDR), r24
wait_xmit:
    in r24, _SFR_IO_ADDR(SPSR)
    sbrs r24, SPIF
    rjmp wait_xmit
	ret
.endfunc

;---------------------------------------------------------------------------;
; AudioRoutine
;---------------------------------------------------------------------------;
; void AudioRoutine();

.global AudioRoutine
.func AudioRoutine
AudioRoutine:
    push	ZL				;
	push	ZH				;/

	lds	ZL, FifoRi			;Get FIFO read index
	clr	ZH				;Z = pointer to the top of FIFO
	subi	ZL, lo8(-(Buff))		;
	sbci	ZH, hi8(-(Buff))		;/
	lds	r24, FifoCt			;Load FIFO data counter

#if MODE >= 1	// Dual output
	subi	r24, 2				;Check availability of the sampling data
	brcs	9f				;/
	sts	FifoCt, r24			;Save FIFO data counter
	ld	r24, Z+				;Get R-ch/LSB data and send it to PWM
	out	_SFR_IO_ADDR(OCR0A), r24	;/
	ld	r24, Z+				;Get L-ch/MSB data and send it to PWM
	out	_SFR_IO_ADDR(OCR0B), r24	;/
#else		// Single output
	subi	r24, 1				;Check availability of the sampling data
	brcs	9f				;/
	sts	FifoCt, r24			;Save FIFO data counter
	ld	r24, Z+				;Send data to PWM
	out	_SFR_IO_ADDR(OCR0B), r24	;/
#endif
	subi	ZL, lo8(Buff)			;Save FIFO read index
	sts	FifoRi, ZL			;/

9:	pop	ZH				;Restore regs.
	pop	ZL				;
	ret
.endfunc

;---------------------------------------------------------------------------;
; Read and forward a partial data block
;
; void fwd_blk_part (void*, WORD, WORD);

.global fwd_blk_part
.func fwd_blk_part
fwd_blk_part:
	movw	XL, r24			;X = R25:R24 (memory address)
	movw	ZL, r22			;Z = R23:R22 (byte offset in the sector)

	ldi	r18, lo8(514)		;R19:R18 = 514, Number of bytes to receive
	ldi	r19, hi8(514)		;/
	sub	r18, ZL			;R19:R18 -= Z
	sbc	r19, ZH			;/
	sub	r18, r20		;R19:R18 -= R21:R20
	sbc	r19, r21		;/
	; Skip leading data bytes
1:	sbiw	ZL, 1			;Skip leading data...
	brcs	2f			;
	.rept 16			;Discard a byte on SPI
	.endr				;/
	rjmp	1b			;
2:	sbiw	XL, 0			;Destination?
	breq	fb_wave

fb_mem:	; Store intermediate data bytes to the memory
	rcall	xmit_spi	    ;do
	st	X+, r24			    ; *X++ = rcv_spi()
	subi	r20, 1			;while (--r21:r20)
	sbci	r21, 0			;
	brne	fb_mem			;/
	rjmp	9b

fb_wave: ; Forward intermediate data bytes to the wave FIFO
	sbic	_FLAGS, 4		;if (16bit data) R21:R20 /= 2;
	lsr	r21			;
	sbic	_FLAGS, 4		;
	ror	r20			;/
	sbic	_FLAGS, 1		;if (Stereo data) R21:R20 /= 2;
	lsr	r21			;
	sbic	_FLAGS, 1		;
	ror	r20			;/
	lds	r22, FifoWi		;r22 = FIFO write index

3:	ldi	XL, lo8(Buff)		;X = Buff + R22
	ldi	XH, hi8(Buff)		;
	add	XL, r22			;
	adc	XH, r1			;/

4:	lds	r24, FifoCt		;wait while FIFO full
	cpi	r24, 252		;
	brcc	4b			;/
	sbic	_FLAGS, 4		;Get L-ch/Mono data into ZL
	rcall	xmit_spi			;
	rcall	xmit_spi			;
	sbic	_FLAGS, 4		;
	subi	r24, 0x80		;
	mov	ZL, r24			;/
	sbis	_FLAGS, 1		;if Mono data, do not process R-ch data
	rjmp	8f			;/
	sbic	_FLAGS, 4		;Get R-ch data
	rcall	xmit_spi			;
	rcall	xmit_spi			;
	sbic	_FLAGS, 4		;
	subi	r24, 0x80		;/
	add	ZL, r24			;ZL = (ZL + R-ch) / 2
	ror	ZL			;/

8:	st	X+, ZL			;Store ZL into FIFO
	cli				;
	lds	r24, FifoCt		;
	inc	r24			;
	sts	FifoCt, r24		;
	sei				;
	inc	r22			;/

	subi	r20, lo8(1)		;while(--R21:R20)
	sbci	r21, hi8(1)		;
	brne	3b			;/
	sts	FifoWi, r22		;Save FIFO write index

9:	.rept 16			;Discard a byte on SPI
	.endr				;/
	subi	r18, lo8(1)		;Repeat r19:r18 times
	sbci	r19, hi8(1)		;
	brne	9b			;/

	ret
.endfunc

